#include <iostream>
#include <array>
#include <chrono>
#include <thread>
#include <cstdint>
#include <fcntl.h>
#include <unistd.h>
#include <linux/i2c-dev.h>
#include <sys/ioctl.h>

// Constants
const uint8_t IST8310_ADDRESS = 0x0E;
const char *I2C_BUS = "/dev/i2c-1";
const uint8_t REG_WAI = 0x00;
const uint8_t REG_DATA_X = 0x03;
const uint8_t REG_DATA_TEMP_L = 0x1C;
const uint8_t REG_CTRL1 = 0x0A;
const uint8_t REG_CTRL2 = 0x0B;
const uint8_t REG_AVG_CTRL = 0x41;
const uint8_t REG_SENS_MODE_SELECT = 0x42;
const uint8_t AVG_CTRL_16X = 0x24;
const uint8_t SRPD_MODE_LOW_POWER = 0xC0;
const size_t BLOCK_BUF_LEN = 32;

class I2CDevice
{
public:
  virtual void write(uint8_t address, const uint8_t *data, size_t length) = 0;
  virtual void read(uint8_t address, uint8_t *data, size_t length) = 0;
  virtual void writeRead(uint8_t address, const uint8_t *tx_data, size_t tx_length, uint8_t *rx_data, size_t rx_length) = 0;
};

class LinuxI2C : public I2CDevice
{
public:
  LinuxI2C(const char *device) { fd = open(device, O_RDWR); }
  ~LinuxI2C()
  {
    if (fd >= 0)
      close(fd);
  }
  void write(uint8_t address, const uint8_t *data, size_t length) override
  {
    ioctl(fd, I2C_SLAVE, address);
    ::write(fd, data, length);
  }
  void read(uint8_t address, uint8_t *data, size_t length) override
  {
    ioctl(fd, I2C_SLAVE, address);
    ::read(fd, data, length);
  }
  void writeRead(uint8_t address, const uint8_t *tx_data, size_t tx_length, uint8_t *rx_data, size_t rx_length) override
  {
    write(address, tx_data, tx_length);
    read(address, rx_data, rx_length);
  }

private:
  int fd;
};

class IST8310
{
public:
  IST8310(I2CDevice &i2c_device) : i2c_device(i2c_device), address(IST8310_ADDRESS) { initSensor(); }
  void continuousRead()
  {
    while (true)
    {
      auto mag_vector = getMagVector();
      int16_t temperature = getTemperature();
      std::cout << "Magnetometer vector: X=" << mag_vector[0]
                << " Y=" << mag_vector[1]
                << " Z=" << mag_vector[2]
                << std::endl;
      //   << " Temperature: " << temperature << " (raw value)" << std::endl;
    }
  }

private:
  I2CDevice &i2c_device;
  uint8_t address;
  std::array<uint8_t, BLOCK_BUF_LEN> block_buf;

  void initSensor()
  {
    writeRegister(REG_CTRL2, 0x01);
    writeRegister(REG_AVG_CTRL, AVG_CTRL_16X);
    writeRegister(REG_SENS_MODE_SELECT, SRPD_MODE_LOW_POWER);
  }

  std::array<int16_t, 3> getMagVector()
  {
    writeRegister(REG_CTRL1, 0x01);
    std::this_thread::sleep_for(std::chrono::milliseconds(6));
    readBlock(REG_DATA_X, 6);
    return {toInt16(0), toInt16(2), toInt16(4)};
  }

  int16_t getTemperature()
  {
    readBlock(REG_DATA_TEMP_L, 2);
    return toInt16(0);
  }

  void readBlock(uint8_t reg, size_t recv_count)
  {
    uint8_t cmd_buf[1] = {reg};
    i2c_device.writeRead(address, cmd_buf, 1, block_buf.data(), recv_count);
  }

  void writeRegister(uint8_t reg, uint8_t val)
  {
    uint8_t data[2] = {reg, val};
    i2c_device.write(address, data, 2);
  }

  uint8_t readRegister(uint8_t reg)
  {
    readBlock(reg, 1);
    return block_buf[0];
  }

  int16_t toInt16(size_t idx)
  {
    return static_cast<int16_t>(block_buf[idx]) | (static_cast<int16_t>(block_buf[idx + 1]) << 8);
  }
};

// int main()
// {
//   LinuxI2C i2c(I2C_BUS);
//   IST8310 sensor(i2c);
//   sensor.continuousRead();
//   return 0;
// }
